function [model,errorMes] = solveQTSM_smoothing(params,setup)

%% Allocating memory 
maxMat     = max(setup.matSelect);            % max maturity
ny         = size(setup.matSelect,2);         % number of observables
nx         = setup.nx;                        % number of factors
g0         = zeros(ny,1);                     % y = g(x) 
gx         = zeros(ny,nx);
gxx        = zeros(ny,nx^2);
%h0         = zeros(nx,1);                     % x_t+1 = h0 + hx*x_t under P
%hx         = zeros(nx,nx);

% Unfold parameters in params to get the QTSM
if nx == 1
    alpha   = params.alpha;
    psi     = 1;   
	beta    = [params.beta1]';
    phi     = [params.phi11];
    mu      = [params.mu1     ]';
    sigma   = [params.sigma11 ];
elseif nx == 2
    alpha   = params.alpha;
    psi     = ones(nx,nx);   
	beta    = [params.beta1  params.beta2  ]';
    phi     = [params.phi11  params.phi12   ;
               0             params.phi22  ];
    mu      = [params.mu1    params.mu2    ]';
    sigma   = [params.sigma11  0                 ;
               params.sigma21  params.sigma22    ];
elseif nx == 3
    alpha   = params.alpha;
    psi     = ones(nx,nx);
    beta    = [params.beta1  params.beta2   params.beta3]';
    phi     = [params.phi11   params.phi12  params.phi13;
               0              params.phi22  params.phi23;
               0              0             params.phi33];
    mu      = [params.mu1     params.mu2    params.mu3]';
    sigma   = [params.sigma11  0                 0              ;
               params.sigma21  params.sigma22    0              ;
               params.sigma31  params.sigma32    params.sigma33];
elseif nx == 4
    alpha   = params.alpha;
    psi     = ones(nx,nx);   
    beta    = [params.beta1  params.beta2   params.beta3   params.beta4]';
    phi     = [params.phi11  params.phi12   params.phi13   params.phi14;
               0             params.phi22   params.phi23   params.phi24;
               0             0              params.phi33   params.phi34;
               0             0              0              params.phi44];
    mu      = [params.mu1    params.mu2     params.mu3     params.mu4]';
    sigma   = [params.sigma11       0                  0                0;
               params.sigma21       params.sigma22     0                0;
               params.sigma31       params.sigma32     params.sigma33   0;
               params.sigma41       params.sigma42     params.sigma43   params.sigma44];
elseif nx == 5
    alpha   = params.alpha;
    psi     = ones(nx,nx);   
    beta    = [params.beta1  params.beta2   params.beta3   params.beta4  params.beta5]';
    phi     = [params.phi11  params.phi12   params.phi13   params.phi14  params.phi15;
               0             params.phi22   params.phi23   params.phi24  params.phi25;
               0             0              params.phi33   params.phi34  params.phi35;
               0             0              0              params.phi44  params.phi45;
               0             0              0              0             params.phi55] ;
    mu      = [params.mu1    params.mu2     params.mu3     params.mu4    params.mu5]';
    sigma   = [params.sigma11       0                  0                0               0;
               params.sigma21       params.sigma22     0                0               0;
               params.sigma31       params.sigma32     params.sigma33   0               0;
               params.sigma41       params.sigma42     params.sigma43   params.sigma44  0;
               params.sigma51       params.sigma52     params.sigma53   params.sigma54  params.sigma55];
end

% Check psi is positive semi-definite or alpha is negative
if alpha < 0
    errorMes = 1;
    model = [];
    return
end

% Normalization restrictions
for i=1:nx-1
    if phi(i,i) > phi(i+1,i+1)
        errorMes = 1;
        model = [];
        return
    end
    % The Jordan form: normalization with (near identical eigenvalues under Q
    phi(i,i+1) = (1-abs(phi(i,i)-phi(i+1,i+1)))^5000;
end

% Law of motion for factors under Q 
h0Q = phi*mu;
hxQ = eye(nx)-phi;

% Law of motion for factors under P
%h0  = h0Q + f0;     %note MPR is scaled by sigma
%hx  = hxQ + fx;
h0 = zeros(nx,1);
hx = zeros(nx,nx);
for i=1:nx
    name = ['h0',num2str(i)];
    h0(i,1) = params.(name);
end
for i=1:nx
    for j=1:nx
        name = ['hx',num2str(i),num2str(j)];
        hx(i,j) = params.(name);
    end
end

% Check if co-variance matrix for innovations is positive semi-definite
sigma2 = sigma*sigma';
if any(eig(sigma2)<0)      % used for dataOption 1
%if any(eig(sigma2)<1e-10)
    errorMes = 1;
    model = [];
    return
end
%% Compute bond pricing coefficients
% Initialise memory for bond price coefficients in state 1
A1 = zeros(1,maxMat);
B1 = zeros(nx,maxMat);
C1 = zeros(nx,nx,maxMat);

% % Compute the inverse of sigma squared (sigma is diagonal)
% % invsigma2 = diag(1./diag(sigma.^2));
% % for non-diagonal case
% invsigma2 = sigma2\eye(nx);
% if any(isnan(reshape(invsigma2,nx^2,1))==1) || any(isinf(reshape(invsigma2,nx^2,1))==1)
%     errorMes = 1;
%     model = [];
%     return
% end
% detSigma  = det(sigma);

% Compute recursive coefficients for bond prices
for n = 1:maxMat   
    % One-period bond prices use the initial conditions
    if n == 1
        % Compute the recursive bond pricing coefficients.  All terms
        % involving multiplication by A0, B0 or C0 are zero.
        A1(1,n) = - alpha;
        B1(:,n) = - beta;
        C1(:,:,n) = - psi;

    % Longer bond prices depend on the previous maturity
    else
        gamma    = eye(nx)-2*sigma'*C1(:,:,n-1)*sigma;
        invgamma = inv(gamma);
        A1(1,n) = A1(1,n-1) - alpha + B1(:,n-1)'*phi*mu + mu'*phi'*C1(:,:,n-1)*phi*mu ...
            + 0.5*B1(:,n-1)'*sigma*invgamma*sigma'*B1(:,n-1) ...
            + 2*(phi*mu)'*C1(:,:,n-1)*sigma*invgamma*sigma'*B1(:,n-1)...
            + 2*(phi*mu)'*C1(:,:,n-1)*sigma*invgamma*sigma'*C1(:,:,n-1)'*(phi*mu) ...
            - 0.5*log(det(gamma));       
        
        B1(:,n) = (B1(:,n-1)'*(eye(nx)-phi)-beta'...
                    +2*mu'*phi'*C1(:,:,n-1)*(eye(nx)-phi)...
                    +2*B1(:,n-1)'*sigma*invgamma*sigma'*C1(:,:,n-1)'*(eye(nx)-phi)...
                    +4*(phi*mu)'*C1(:,:,n-1)*sigma*invgamma*sigma'*C1(:,:,n-1)'*(eye(nx)-phi))';
        C1(:,:,n) = (eye(nx)-phi)'*C1(:,:,n-1)*(eye(nx)-phi) - psi ...
                    + 2*(eye(nx)-phi)*C1(:,:,n-1)*sigma*invgamma*sigma'*C1(:,:,n-1)'*(eye(nx)-phi);

    end
end


%% Reporting output in the desired format: annualized yields in pct.
% Spot: y_{n,t} = -(1/n)*p_{n,t}
% The short rate
r0   = 12*(-A1(1)/1);
rx   = 12*(-B1(:,1)'/1);
rxx  = 12*(-reshape(C1(:,:,1),1,nx^2)/1);
g0   = 12*(-A1(1,setup.matSelect)'./setup.matSelect');
gx   = 12*(-B1(:,setup.matSelect)'./repmat(setup.matSelect',1,nx));
for i=1:ny
    gxx(i,:) = 12*(-reshape(C1(:,:,setup.matSelect(1,i)),nx^2,1)./repmat(setup.matSelect(1,i),1,nx^2)');
end

% Storing output
model = struct('g0',g0,'gx',gx,'gxx',gxx,'h0Q',h0Q,'hxQ',hxQ,'h0',h0,'hx',hx,'sigma',sigma,'matSelect',setup.matSelect,...
    'r0',r0,'rx',rx,'rxx',rxx,'A1',A1,'B1',B1,'C1',C1,'alpha',alpha,'beta',beta,'psi',psi,'phi',phi);

% Report no error if applicable
errorMes = 0;


end